/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <opencv/highgui.h>
#include <iostream>

#include "vision/geomtypes.h"
#include "ckatanawitheye.h"
#include "katanafuncs.h"

#define TIMEOUT 10000

namespace Katana {

CKatanaWithEye::CKatanaWithEye(CikBase& _katana, CImageSource& _cam, CPointDetector& det, CCamTransform& _tr)
: cam(_cam), katana(_katana), detector(det), transform(_tr) {
	recalibration_path.push_back(cv::Point3f(50, 50, 10));
	recalibration_path.push_back(cv::Point3f(-50, 50, 30));
	recalibration_path.push_back(cv::Point3f(-50, -50, 10));
	recalibration_path.push_back(cv::Point3f(50, -50, 30));
	shift_recal_path = true;
}


CKatanaWithEye::~CKatanaWithEye()
{
}

void CKatanaWithEye::Calibrate() {
	Path3D points;
	
	points.push_back(cv::Point3d(400, 0, -20));
	
	points.push_back(cv::Point3d(480, -150, -80));
	points.push_back(cv::Point3d(500, 0, -80));
	points.push_back(cv::Point3d(480, 150, -80));
	
	points.push_back(cv::Point3d(400, 100, -80));
	points.push_back(cv::Point3d(400, 0, -80));
	points.push_back(cv::Point3d(400, -100, -80));
	
	points.push_back(cv::Point3d(250, -100, -80));
	points.push_back(cv::Point3d(250, 100, -80));
	
	points.push_back(cv::Point3d(300, -150, -40));
	points.push_back(cv::Point3d(300, 0, -40));
	points.push_back(cv::Point3d(300, 150, -40));
	
	Calibrate(points);
}

void CKatanaWithEye::Calibrate(const Path3D& points) {
	cv::Point2f i_point;
	IplImage * img;
	float r, delta;
	Katana::SActuatorPosition p;
	cv::Point3f o;
	
	cvNamedWindow("calibrate", 0);
	
	Path3D rpath;
	std::vector<cv::Point2f> ipath;
	
	for(Path3D::const_iterator o_point = points.begin(); o_point != points.end(); o_point++) {
		katana.moveRobotTo(o_point->x, o_point->y, o_point->z, 0.0, 0.0, 0.0, true, TIMEOUT);
		
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		r = sqrtf(p.x*p.x + p.y*p.y);
		delta = (r - 10*sinf(p.phi)) / r;
		o.x = o_point->x*delta; 
		o.y = o_point->y*delta;
		o.z = o_point->z + 10*cosf(p.phi);
		
		for(int i = 0; i < 10; i++) {
			img = cam.GrabFrame();
			cvWaitKey(200);
		}
		if (detector.DetectPoint(img, i_point)) {
			rpath.push_back(o);
			ipath.push_back(i_point);
			
			std::cout << i_point << " -> " << o << *o_point << std::endl;
			cvCircle(img, i_point, 3, cvScalar(0,255,0), -1);
			cvShowImage("calibrate", img);
			if (-1 == cvWaitKey(1000)) transform.AddPair(i_point, o);
		}
	}
	
	transform.CalcTransform();
	transform.PrintTParams();
	
	img = cam.GrabFrame();
	for(int i = 0; i < ipath.size(); i++) {
		std::cout << ipath[i] << " <-> ";
		cvCircle(img, ipath[i], 2, cvScalar(0,255,255), -1);
		i_point = transform.ProjectPoint(rpath[i]);
		std::cout << i_point << std::endl;
		cvCircle(img, i_point , 2, cvScalar(255,0,0), -1);
		
	}
	cvShowImage("calibrate", img);
	cvWaitKey(0);
	
	cvDestroyWindow("calibrate");
}

void CKatanaWithEye::ReCalibrate(const Path3D& path, bool shift_path) {
	cv::Point2f i_point;
	cv::Point3f o_point, start_point;
	double phi, theta, psi, x, y, z;
	IplImage * img;
	
	katana.getCoordinates(x, y, z, phi, theta, psi, true);
	start_point = cv::Point3f(x, y, z);
	
	for(Path3D::const_iterator i = path.begin(); i != path.end(); i++) {
		o_point = *i;
		if (shift_path) o_point += start_point;
		
		katana.moveRobotTo(o_point.x, o_point.y, o_point.z - 12.5, 0.0, 0.0, 0.0, true, TIMEOUT);
		
		img = cam.GrabFrame();
		if (detector.DetectPoint(img, i_point)) {
			std::cout << i_point << " -> " << o_point << std::endl;
			transform.NewPair(i_point, o_point);
		}
	}
	
	transform.CalcTransform();
}

bool CKatanaWithEye::CheckTransform(float threshold, const Path3D& path, bool shift_path, bool recalibrate) {
	bool need_recalibrate;
	cv::Point2f img_pt;
			
	if (detector.DetectPoint(cam.GrabFrame(), img_pt)) {
		
		cv::Point3f obj_pt, obj_img_pt;
		double phi, theta, psi, x, y, z;
		
		katana.getCoordinates(x, y, z, phi, theta, psi, true);
		obj_pt.x = x; obj_pt.y = y; obj_pt.z = z;
		
		transform.NewPair(img_pt, obj_pt);
	
		obj_img_pt = transform.ReverseProjection(img_pt, HPlane3f(0, 0, -1, obj_pt.z));
		need_recalibrate = norm(obj_img_pt - obj_pt) > threshold;
		
		std::cout << img_pt << " -> " << obj_pt << ", " << obj_img_pt << std::endl;
		
		if (need_recalibrate && recalibrate) ReCalibrate(path, shift_path);
		else transform.RecalcTransform();
		
		return need_recalibrate;
	}
	else return true;
}

bool CKatanaWithEye::CheckTransform(float threshold, bool recalibrate) {
	return CheckTransform(threshold, recalibration_path, shift_recal_path, recalibrate);
}
	
void CKatanaWithEye::movToImagePoint(const cv::Point2f& point, const HPlane3f& plane, double theta, bool wait, int waittime) {
	cv::Point3f o_point = transform.ReverseProjection(point, plane);
	std::cout << point << " -> " << o_point << std::endl;
	katana.moveRobotTo(o_point.x, o_point.y, o_point.z, 0.0, theta, 0.0, true, waittime);
}

}
